#include "s_xpt2046.h"

#include <stdio.h>
#include <string.h>

#define S_XPT2046_Z1_AD_REG                         0xB1
#define S_XPT2046_Z2_AD_REG                         0xC1
#define S_XPT2046_X_AD_REG                          (0xD0)
#define S_XPT2046_Y_AD_REG                          (0x90)

#define S_XPT2046_GET_NUM                           10
#define S_XPT2046_GET_RID_OF_NUM                    1

// #define S_XPT2046_BOARDER_NUM                       (S_XPT2046_GET_RID_OF_NUM / S_XPT2046_GET_NUM)

#define S_XPT2046_X_MIN                             1
#define S_XPT2046_X_MAX                             5000
#define S_XPT2046_Y_MIN                             1
#define S_XPT2046_Y_MAX                             5000

#define S_XPT2046_MAX_ERROR                         3000

static void s_xpt2046_order(signed short *data)
{
    unsigned short temp = 0;
    unsigned short i, j;
    for(i = 1; i < data[0] - 2; i++)
    {
        for(j = 1; j < data[0] - 2 - i; j++)
        {
            if(data[j] > data[j+1])
            {
                temp = data[j];
                data[j] = data[j+1];
                data[j+1] = temp;
            }
        }
    }
}

static unsigned short s_xpt2046_get_data(unsigned short *data, unsigned short boarder_num)
{
    unsigned int final = 0;
    unsigned short i = 0;

    for(i = boarder_num; i < data[0] - boarder_num; i++)
    {
        final += data[i];
    }

    return (unsigned short)(final / (data[0] - 2 * boarder_num));
}


// static unsigned short s_xpt2046_getavrg(signed short x, signed short y, signed short z)
// {
//     signed int da, db, dc;
//     unsigned short reta = 0;
//     if ( x > y ) da = x - y; else da = y - x;
//     if ( x > z ) db = x - z; else db = z - x;
//     if ( z > y ) dc = z - y; else dc = y - z;

//     if ( da <= db && da <= dc ) reta = (x + y) >> 1;
//     else if ( db <= da && db <= dc ) reta = (x + z) >> 1;
//     else reta = (y + z) >> 1;   //    else if ( dc <= da && dc <= db ) reta = (x + y) >> 1;

//     return (reta);
// }

// static unsigned short s_xpt2046_send_get(s_xpt2046_t *xpt, unsigned char send)
// {
//     unsigned char cmd[1] = {send};
//     unsigned char data[2];

//     xpt->write(cmd, 1);
//     xpt->read(data, 2);


//     return (unsigned short)(data[0] << 5) | (unsigned short)(data[1] >> 3);
// }

// s_xpt2046_state s_xpt2046_read_raw_pos_new(s_xpt2046_t *xpt)
// {
//     unsigned short data[6], z1, z2;
//     signed int z;


//     if(xpt->cs_enable == NULL)
//     {
//         return s_xpt2046_cs_enable_null;
//     }
//     if(xpt->cs_disable == NULL)
//     {
//         return s_xpt2046_cs_disable_null;
//     }
//     if(xpt->write == NULL)
//     {
//         return s_xpt2046_write_null;
//     }
//     if(xpt->read == NULL)
//     {
//         return s_xpt2046_read_null;
//     }
//     if(xpt->delay_us == NULL)
//     {
//         return s_xpt2046_delay_null;
//     }


//     xpt->cs_enable();
//     z1 = s_xpt2046_send_get(xpt, 0xB1);
//     // z = z1 + 4095;
//     z2 = s_xpt2046_send_get(xpt, 0xC1);
//     // z -= z2;

//     z = z1 + 4095 - z2;


//     // printf("%d\r\n", z);


//     if(z >= 400)
//     {
//         // cmd[0] = 0x91;
//         // xpt->write(cmd, 1);
//         // xpt->read(cmd, 1);
//         s_xpt2046_send_get(xpt, 0x91);

//         data[0] = s_xpt2046_send_get(xpt, 0xD1);
//         data[1] = s_xpt2046_send_get(xpt, 0x91);
//         data[2] = s_xpt2046_send_get(xpt, 0xD1);
//         data[3] = s_xpt2046_send_get(xpt, 0x91);

//     }
//     else
//     {
//         data[0] = data[1] = data[2] = data[3] = 0;
//     }

//     // xpt->cs_disable();

//     // xpt->cs_enable();
//     data[4] = s_xpt2046_send_get(xpt, 0xD1);
//     // xpt->cs_disable();
//     // xpt->cs_enable();
//     data[5] = s_xpt2046_send_get(xpt, 0x90);

//     xpt->cs_disable();

    

//     if(z < 0)
//     {
//         z = 0;
//     }
//     if(z < 400 || z > 3000)
//     {
//         xpt->raw_z = 0;
//         // if(z < 75)
//         // {
//             // printf("z: %d\r\n", z);
//         // printf("%d, %d, %d\r\n", z1, z2, z);
//         //     return s_xpt2046_z_error;
//         // }
//         // return s_xpt2046_z_error;
//     }

//     xpt->raw_z = z;

//     // printf("%d, %d, %d\r\n", z1, z2, z);

//     xpt->raw_x = data[4];//s_xpt2046_getavrg(data[0], data[2], data[4]);
//     xpt->raw_y = data[5]; //s_xpt2046_getavrg(data[1], data[3], data[5]);

//     printf("%d, %d, %d\r\n", xpt->raw_x, xpt->raw_y, xpt->raw_z);

//     // printf("%d, %d\r\n", 1, 0);

//     // printf("get\r\n");

//     return 0;

// }

s_xpt2046_state s_xpt2046_read_raw_pos(s_xpt2046_t *xpt)
{
    unsigned char cmd[4] = {S_XPT2046_X_AD_REG, S_XPT2046_Y_AD_REG, S_XPT2046_Z1_AD_REG, S_XPT2046_Z2_AD_REG};
    unsigned char xdata[2], ydata[2];

    unsigned short i = 0;
    unsigned short pxs[S_XPT2046_GET_NUM + 1], pys[S_XPT2046_GET_NUM + 1];

    unsigned short z1, z2;
    signed int z;

    unsigned short record_data_x, record_data_y;
    unsigned char show_data_enable = 0;

    if(xpt->cs_enable == NULL)
    {
        return s_xpt2046_cs_enable_null;
    }
    if(xpt->cs_disable == NULL)
    {
        return s_xpt2046_cs_disable_null;
    }
    if(xpt->write == NULL)
    {
        return s_xpt2046_write_null;
    }
    if(xpt->read == NULL)
    {
        return s_xpt2046_read_null;
    }
    if(xpt->delay_us == NULL)
    {
        return s_xpt2046_delay_null;
    }

    xpt->cs_enable();
    xpt->write(&cmd[2], 1);
    xpt->read(&xdata[0], 2);
    xpt->cs_disable();

    xpt->cs_enable();
    xpt->write(&cmd[3], 1);
    xpt->read(&ydata[0], 2);
    xpt->cs_disable();

    z1 = (unsigned short)(xdata[0] << 5) | (unsigned short)(xdata[1] >> 3);
    z2 = (unsigned short)(ydata[0] << 5) | (unsigned short)(ydata[1] >> 3);
    

    // z1 = s_xpt2046_get_data(pz1s, 2);
    // z2 = s_xpt2046_get_data(pz2s, 2);

    // z = z1 + 4096 - z2;
    z = z2 - z1;

    pxs[0] = 1;
    pys[0] = 1;

    for(i = 0; i < S_XPT2046_GET_NUM; i++)
    {

        
        xpt->cs_enable();
        xpt->write(&cmd[1], 1);
        xpt->read(&ydata[0], 2);        // Dummy read
        xpt->cs_disable();


        

        xpt->cs_enable();
        xpt->write(&cmd[0], 1);
        xpt->read(&xdata[0], 2);
        xpt->cs_disable();

        xpt->cs_enable();
        xpt->write(&cmd[1], 1);
        xpt->read(&ydata[0], 2);
        xpt->cs_disable();

        show_data_enable = 0;
    
        record_data_x = pxs[pxs[0]] = (unsigned short)(xdata[0] << 5) | (unsigned short)(xdata[1] >> 3);
        // z1 = (unsigned short)(xdata[0] << 5) | (unsigned short)(xdata[1] >> 3);
        // pxs[pxs[0]] = z1;
        // pxs[pxs[0]] = xdata[0];
        if(pxs[pxs[0]] > S_XPT2046_X_MIN && pxs[pxs[0]] < S_XPT2046_X_MAX)
        {
            show_data_enable = 1;
            pxs[0]++;
        }
        else
        {
            show_data_enable = 0;
        }

        record_data_y = pys[pys[0]] = (unsigned short)(ydata[0] << 5) | (unsigned short)(ydata[1] >> 3);
        // z2 = (unsigned short)(ydata[0] << 5) | (unsigned short)(ydata[1] >> 3);
        // pys[pys[0]] = z2;
        // pys[pys[0]] = ydata[0];
        if(pys[pys[0]] > S_XPT2046_Y_MIN && pys[pys[0]] < S_XPT2046_Y_MAX)
        {
            pys[0]++;
        }
        else
        {
            show_data_enable = 0;
        }

        // if(show_data_enable == 1)
        // {
            // printf("%d, %d\r\n", record_data_x, record_data_y);
        // }
    }

    

    // xpt->cs_enable();

    if(pxs[0] < 2 * S_XPT2046_GET_RID_OF_NUM)
    {
        return s_xpt2046_xoutof_range;
    }
    if(pys[0] < 2 * S_XPT2046_GET_RID_OF_NUM)
    {
        return s_xpt2046_youtof_range;
    }

    // printf("%d, %d\r\n", pxs[0], pys[0]);
    
    s_xpt2046_order(pxs);
    s_xpt2046_order(pys);

    // if(pxs[pxs[0] - 1] - pxs[1] >= S_XPT2046_MAX_ERROR)
    // {
    //     printf("x:%d\r\n", pxs[pxs[0] - 1] - pxs[1]);
    //     return s_xpt2046_max_error;
    // }

    // if(pys[pys[0] - 1] - pys[1] >= S_XPT2046_MAX_ERROR)
    // {
    //     printf("y:%d\r\n", pys[pys[0] - 1] - pys[1]);
    //     return s_xpt2046_max_error;
    // }

    xpt->raw_x = s_xpt2046_get_data(pxs, (S_XPT2046_GET_RID_OF_NUM * (pxs[0] - 1)) / S_XPT2046_GET_NUM);
    xpt->raw_y = s_xpt2046_get_data(pys, (S_XPT2046_GET_RID_OF_NUM * (pys[0] - 1)) / S_XPT2046_GET_NUM);

    // printf("%d, %d, %d, %d, %d, %d\r\n", xpt->raw_x, xpt->raw_y, z1, z2, z, z1 + 512 - z2);

    // if(z > 1000)
    // {
    //     // printf("%d, %d\r\n", xpt->raw_x, xpt->raw_y);
    // }
    // else
    // {
    //     // printf("%d, %d, %d, %d, %d\r\n", xpt->raw_x, xpt->raw_y, z1, z2, z);
    //     return s_xpt2046_z_error;
        
    // }


    
    // printf("%d, %d, %d, %d, %d\r\n", xpt->raw_x, xpt->raw_y, z1, z2, z);

    // if(xpt->raw_x < S_XPT2046_X_MIN || xpt->raw_x > S_XPT2046_X_MAX)
    // {
    //     return s_xpt2046_xoutof_range;
    // }

    // if(xpt->raw_y < S_XPT2046_Y_MIN || xpt->raw_y > S_XPT2046_Y_MAX)
    // {
    //     return s_xpt2046_youtof_range;
    // }

    return s_xpt2046_ok;
}